import lejos.nxt.*;
import lejos.robotics.objectdetection.*;

public class TestSensors implements FeatureListener {

	public static int MAX_DETECT = 80;
	
	public static void main(String[] args) throws Exception {
		
		TestSensors listener = new TestSensors();
		UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
		RangeFeatureDetector fd = new RangeFeatureDetector(us, MAX_DETECT, 500);
		fd.addListener(listener);
		Button.ENTER.waitForPressAndRelease();
	}
	
	public void featureDetected(Feature feature, FeatureDetector detector) {

		int r = 0;
		
		int range = (int)feature.getRangeReading().getRange();
		int ranges = feature.getRangeReadings().getNumReadings();

		for(int i = 0; i < ranges; i++){
			if((int)feature.getRangeReadings().getRange(i) > r) r = (int)feature.getRangeReadings().getRange(i);
		}
		
		Sound.playTone(1200 - (range * 10), 100);
		LCD.drawString("Closest range:" + range, 0, 0);
		LCD.drawString("Num ranges:" + ranges, 0, 1);
		LCD.drawString("Farthest range:" + r, 0, 1);
	}
}